/*
 * Test.cpp
 *
 * Created: 2012-05-08 16:10:09
 *  Author: Johan
 */ 

#include <avr/io.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <avr/interrupt.h>
#include <stdarg.h>
#include "ADConverter.h"
#include "UART.h"
#include "Timer.h"
#include "PWM.h"
#include "IRSensor.h"
#include "Car.h"

UART uart;
PWM pwm;
ADConverter adc;

Car car(pwm, uart);
IRSensor sensor(pwm, &car, uart);

ISR(TIMER1_CAPT_vect){}

ISR(ADC_vect){
	cli();
	sensor.detectPeak();
	sei();
}

bool isCountingTimerUp(){
	
	int first = TCNT3;
	int second = TCNT3;
	while(first == second){
		second = TCNT3;
	}
	
	if(first < second){
		return true;
	} else {
		return false;
	}
}

bool even = false;

ISR(TIMER3_COMPA_vect) {
	cli();

	if(even){
		sensor.move();
		car.move();
		even = false;
		return;
	}
	even = true;
	sei();
}

char* toString(const char* format ...){
	char data[100];

	va_list ap;
	va_start(ap, format);
	vsprintf(data,format,ap);
	va_end(ap);

	return data;
}

int main(void)
{
	uart.send(sizeof(int));
	uart.send("\n\rStarting\n\r");
	uart.send("I'm the Boss!\n\r");
	sei();
	PORTB = 1<<PB3;
	DDRB = 1<<DDB3;
	sensor.setMilliSec(1000);
	pwm.enableCompareInterrupt(pwm.A);
	
	adc.startConversion();
	adc.setAutoTrigger(true);
	
	int timer = TCNT3;
	car.start();
	
	//Sensor
	int sensor_time = timer;
	int sensor_period = 1000;
	
	//Car
	int car_start_time = timer;
	int car_period = 1000;
	
	int temp;
	while(1) {
		int timer = TCNT3;
		
		temp = abs(timer-sensor_time);
		
		if(temp > sensor_period){
			sensor.update(temp);
			sensor_time = timer;
		}
		
		temp = abs(timer-car_start_time);
		
		if(temp > car_period){
			car.update(1);
			car_start_time = timer;
		}
		
		if(car.getState()==car.Running){
			car.setSpeed(1400);
			
		}

		//char ch = uart.receive();
//
		//int width = pwm.getWidth(pwm.B);
		//
		//if(ch == '+'){
			//width += 1;
			//uart.send(width);
			//
		//}else if(ch == '-'){
			//
			//width -= 1;
			//uart.send(width);
			//
		//}
		//pwm.setWidth(pwm.B,width);
		
	}
}

